/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * (C) Copyright 2013
 * David Feng <fenghua@phytium.com.cn>
 */

#include <asm-offsets.h>
#include <config.h>
#include <linux/linkage.h>
#include <asm/macro.h>

.pushsection .text.armv8_switch_to_el2, "ax"
ENTRY(armv8_switch_to_el2)
	switch_el x6, 1f, 0f, 0f
0:
	cmp x5, #ES_TO_AARCH64
	b.eq 2f
	/*
	 * When loading 32-bit kernel, it will jump
	 * to secure firmware again, and never return.
	 */
	bl armv8_el2_to_aarch32
2:
	/*
	 * x4 is kernel entry point or switch_to_el1
	 * if CONFIG_ARMV8_SWITCH_TO_EL1 is defined.
         * When running in EL2 now, jump to the
	 * address saved in x4.
	 */
	br x4
1:	armv8_switch_to_el2_m x4, x5, x6
ENDPROC(armv8_switch_to_el2)
.popsection

.pushsection .text.armv8_switch_to_el1, "ax"
ENTRY(armv8_switch_to_el1)
	switch_el x6, 0f, 1f, 0f
0:
	/* x4 is kernel entry point. When running in EL1
	 * now, jump to the address saved in x4.
	 */
	br x4
1:	armv8_switch_to_el1_m x4, x5, x6
ENDPROC(armv8_switch_to_el1)
.popsection

//.pushsection .text.armv8_a64_to_64nsel1, "ax"
//ENTRY(armv8_a64_to_64nsel1)
//	switch_el x6, 0f, 1f, 0f
//0:
	/* x4 is kernel entry point. When running in EL1
	 * now, jump to the address saved in x4.
	 */
//	br x4
//1:	
//	mov x0, #(1 << 29)	 /* 64bit EL1 | Disable HVC */
//	orr x0,x0, #(1<<31)	
//	msr hcr_el2, x0
//	mov x0, #0x3c5
//	msr spsr_el2, x0		 /*el1h | D | A | I | F */
//	msr elr_el2, x4
//	eret

//ENDPROC(armv8_a64_to_64nsel1)
//.popsection


.pushsection .text.armv8_el2_to_aarch32, "ax"
WEAK(armv8_el2_to_aarch32)
	mov x1, #0x8e0
	
	mov x5, x0
	mov x6, x1
	mov x7, x2
		 
	 switch_el x0, 0f, 1f, 0f
	 0:  ret
	 1:
	 /* Initialize Generic Timers */
	 mrs x0, cnthctl_el2
	 orr x0, x0, #0x3		 /* Enable EL1 access to timers */
	 msr cnthctl_el2, x0
	 msr cntvoff_el2, x0
	 mrs x0, cntkctl_el1
	 orr x0, x0, #0x3		 /* Enable EL0 access to timers */
	 msr cntkctl_el1, x0
	
	 /* Initilize MPID/MPIDR registers */
	 mrs x0, midr_el1
	 mrs x1, mpidr_el1
	 msr vpidr_el2, x0
	 msr vmpidr_el2, x1
		 
	 /* Disable coprocessor traps */
	 mov x0, #0x33ff
	msr cptr_el2, x0		 /* Disable coprocessor traps to EL2 */
	msr hstr_el2, xzr		 /* Disable coprocessor traps to EL2 */
	mov x0, #3 << 20
	msr cpacr_el1, x0		 /* Enable FP/SIMD at EL1 */
	 
	/* Initialize HCR_EL2 */
	mov x0, #(1 << 29) 	 /* 32bit EL1 | Disable HVC */
	msr hcr_el2, x0
		 
	/* SCTLR_EL1 initialization */
	mov x0, #0x0838
	movk	 x0, #0x00c5, lsl #16
	msr sctlr_el1, x0
	
	/* Return to the EL1_SP1 mode from EL2 */
	mov x0, sp
	msr sp_el1, x0 	 /* Migrate SP */
	mrs x0, vbar_el2
	msr vbar_el1, x0		 /* Migrate VBAR */
	mov x0, #0x1d3
	msr spsr_el2, x0		 /* 32bit EL1 System:0x1df/SVC(Supervisor):0x1d3 mode | D | A | I | F */
	
	mov x0, x5
	mov x1, x6
	mov x2, x7
	msr elr_el2, x4
	eret
ENDPROC(armv8_el2_to_aarch32)
.popsection
